/*
 * comath_alg.h
 *
 *  Created on: Apr 29, 2017
 *      Author: yuanjianen
 */

#ifndef _COMATH_ALG_H_
#define _COMATH_ALG_H_

#include "typedef.h"
#include "comath.h"

typedef struct {
	sint32 sum_error;
	sint32 value_of_p;
} PiCtrlState_T;

typedef struct {
	uint16 Kp;
	uint16 Ki;
	sint32 sum_max;
	sint32 sum_min;
	sint32 out_max;
	sint32 out_min;
	uint16 scaling_factor;
} PiCtrlParameter_T;

typedef struct {
	PiCtrlState_T state;
	PiCtrlParameter_T para;
} PiCtrlData_T;

typedef struct {
	sint32 sum_error;
	sint32 value_of_p;
	sint32 sat_diff;
	sint32 result;
} PiAwCtrlState_T;

typedef struct {
	uint16 Kp;
	uint16 Ki;
	uint16 Kb;
	sint32 i_max;
	sint32 i_min;
	sint32 out_max;
	sint32 out_min;
	uint16 scaling_factor;
} PiAwCtrlParameter_T;

typedef struct {
	PiAwCtrlState_T state;
	PiAwCtrlParameter_T para;
} PiAwCtrlData_T;

typedef struct {
	sint32 sum_error;
	sint32 value_of_p;
	sint32 d_pre;
} PidCtrlState_T;

typedef struct {
	uint16 Kp;
	uint16 Ki;
	uint16 Kd;
	sint32 sum_max;
	sint32 sum_min;
	sint32 out_max;
	sint32 out_min;
	uint16 scaling_factor;
} PidCtrlParameter_T;

typedef struct {
	PidCtrlState_T state;
	PidCtrlParameter_T para;
} PidCtrlData_T;

typedef struct {
	sint32 out_hd;
	sint16 coef;			/* LSB: 1/32768 */
} FisrtOrderFilt_T;

typedef enum {
	ENTER_FROM_LO_TO_HI,
	ENTER_FROM_HI_TO_LO
} EnterType;

typedef struct {
	struct {
		bool   result;
		uint16 enter_count;
		uint16 escape_count;
	} control;
	struct {
		EnterType enter_type;
		sint16 thres_hi;
		sint16 thres_lo;
		uint16 enter_counter_max;
		uint16 escape_counter_max;
	} para;
} OutputLimitDelay_T;

#define COMATH_DEGREE_00		(0u)				/* lsb: 2pi/65536 */
#define COMATH_DEGREE_30		(5461u)				/* lsb: 2pi/65536 */
#define COMATH_DEGREE_60		(10923u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_90		(16384u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_120		(21845u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_150		(27307u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_180		(32768u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_210		(38229u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_240		(43691u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_270		(49152u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_330		(60075u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_360		(65535u)			/* lsb: 2pi/65536 */


#define COMATH_DEGREE_5			(910u)				/* lsb: 2pi/65536 */
#define COMATH_DEGREE_10		(1820u)				/* lsb: 2pi/65536 */
#define COMATH_DEGREE_15		(2731u)				/* lsb: 2pi/65536 */
#define COMATH_DEGREE_20		(3641u)				/* lsb: 2pi/65536 */
#define COMATH_DEGREE_25		(4551u)				/* lsb: 2pi/65536 */
#define COMATH_DEGREE_35		(6372u)				/* lsb: 2pi/65536 */
#define COMATH_DEGREE_40		(7282u)				/* lsb: 2pi/65536 */
#define COMATH_DEGREE_45		(8192u)				/* lsb: 2pi/65536 */
#define COMATH_DEGREE_50		(9102u)				/* lsb: 2pi/65536 */
#define COMATH_DEGREE_55		(10012u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_65		(11832u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_70		(12743u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_75		(13653u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_80		(14564u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_100		(18204u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_105		(19114u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_110		(20024u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_115		(20935u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_125		(22755u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_135		(24576u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_130		(23665u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_140		(25486u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_160		(29127u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_165		(30037u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_170		(30947u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_175		(31857u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_185		(33678u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_190		(34588u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_195		(35499u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_200		(36409u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_220		(40050u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_225		(40960u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_230		(41870u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_235		(42780u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_245		(44600u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_250		(45511u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_255		(46421u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_260		(47332u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_280		(50972u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_285		(51883u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_290		(52792u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_295		(53703u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_300		(54613u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_305		(55523u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_310		(56433u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_315		(57344u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_320		(58254u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_340		(61895u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_345		(62805u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_350		(63715u)			/* lsb: 2pi/65536 */
#define COMATH_DEGREE_355		(64625u)			/* lsb: 2pi/65536 */

#define OCTANTIFY(_x, _y, _o)   do {                            \
    sint16 _t; _o= 0;                                           \
    if(_y<  0)  {            _x= -_x;   _y= -_y; _o += 4; }     \
    if(_x<= 0)  { _t= _x;    _x=  _y;   _y= -_t; _o += 2; }     \
    if(_x<=_y)  { _t= _y-_x; _x= _x+_y; _y=  _t; _o += 1; }     \
} while(0);

#define SVM_SHIFT_BITS			(5)

/* theta unit rad, lsb 2pi/65536 */
#define svm_lookup_table(theta)	(comath_svm_table[(theta >> SVM_SHIFT_BITS) & 0x7FF])

extern const sint16 comath_svm_table[];
extern const sint16 sine_table[];
extern const uint16 atan_table[];

void comath_xy_to_polar_coordinates(sint16 x, sint16 y, sint16 *rou, uint16 *theta);


sint16 svm_lookup_table_hd(uint16 theta);
void first_order_filter_init(FisrtOrderFilt_T *first_order_flt, int16 input_init, int16 k);
int16 first_order_filter(FisrtOrderFilt_T *first_order_flt, int16 input);
int32 speed_ramp(int32 dest_speed, int32 curr_speed, int32 accelerate);
int16 linear_interprotion_xint16_yint16(const int16 *x_axis, const int16 *y_axis, uint16 axis_len, int16 x);
int16 linear_interprotion_xuint16_yint16(const uint16 *x_axis, const int16 *y_axis, uint16 axis_len, uint16 x);
uint16 linear_interprotion_xuint16_yuint16(const uint16 *x_axis, const uint16 *y_axis, uint16 axis_len, uint16 x);
void pi_controller_init(PiCtrlData_T* pi_controller, uint16 Kp, uint16 Ki, int16 sum_min, int16 sum_max, int16 out_min, int16 out_max, uint16 scaling_factor);
void pi_controller_reset( PiCtrlData_T* pi_controller, int16 current_value);
int32 pi_controller_calc(PiCtrlData_T* pi, int16 error);
void pi_anti_windup_controller_init(PiAwCtrlData_T* piAw_controller, uint16 Kp, uint16 Ki, uint16 Kb, int16 i_min, int16 i_max, int16 out_min, int16 out_max, uint16 scaling_factor);
void pi_anti_windup_controller_reset( PiAwCtrlData_T* piAw_controller, int16 current_value);
int32 pi_anti_windup_controller_calc(PiAwCtrlData_T* piAw, int16 error);
void pid_controller_init(PidCtrlData_T* pid_controller, uint16 Kp, uint16 Ki, uint16 Kd, int16 sum_min, int16 sum_max, int16 out_min, int16 out_max, uint16 scaling_factor);
void pid_controller_reset( PidCtrlData_T* pid_controller, int16 current_value);
int32 pid_controller_calc(PidCtrlData_T* pid_controller, int16 error);
void comath_output_limit_delay_init(OutputLimitDelay_T *limit_delay, 
		EnterType enter_type,
		sint16 thres_hi, 
		sint16 thres_lo, 
		uint16 enter_counter_max, 
		uint16 escape_counter_max);
bool comath_ouput_limitation_delay_calc(OutputLimitDelay_T *limit_delay, sint16 input);
void comath_sin_cos_lookup_table(sint16 theta, sint16 *sine_value, sint16 *cos_value);
sint16 comath_atan_lookup(sint16 y, sint16 x);

#endif /* _COMATH_ALG_H_ */
